#!/usr/bin/env python
# -*- encoding: UTF-8 -*-
from __future__ import print_function

import cv2
import numpy as np

# Nao 功能模块导入
from . import vision_definitions

# 导入数据库
from .. import db
from .. import flag
from . import singleton
from . import proxyManger

# 相机管理器,是一个单例，全局只有一个
# cm1 = CamManger()
# cm2 = CamManger()
# assert id(cm1) == id (cm2)

@singleton # 用修饰器实现单例
class CamManger(object):
    def __init__(self):
        self._cams_id = []
        self._current_cam_id = None

    def add(self,cam):
        self.add_id(cam.id)

    def add_id(self, cam_id):
        if not isinstance(cam_id,int): raise ValueError,"CamManger: only can add Cam id "
        if cam_id in self._cams_id: print("CamManger: trying to adding a registered cam_id",cam_id)
        cam = Cam.get(cam_id)
        cam.connect()
        self._cams_id.append(cam_id)

    def get(self, cam_id):
        # 获取已经注册的相机（已经打开的相机)
        if cam_id not in self._cams_id:
            print("CamManger: getting a None register camera(ID: %d)",e)
            return None
        return Cam.get(cam_id)

    def get_video(self,cam_id):
        return CamVideo(self.get(cam_id))

    def has(self,cam_id):
        return cam_id in self._cams_id

    def remove(self,cam):
        self.remove_id(cam.id)

    def remove_id(self, cam_id):
        if cam_id not in self._cams_id:
            print("CamManger: removing a None register cam_id %s" % str(cam_id))
        else:
            cam = self.get(cam_id)
            cam.stop()

    def set_current_cam(self,cam):
        self.set_current_id(cam.id)

    def set_current_id(self,cam_id):
        if cam_id not in self._cams_id:
            raise ValueError,"CamManger:set_current_cam: you have to add cam_id %d first" % cam_id
        else:
            self._current_cam_id = cam_id

    def get_current_cam(self,err_callback=None):
        return self.get_current_id(err_callback=err_callback)

    def get_current_id(self,err_callback=None):
        if self._current_cam_id is not None:
            return self.get(self._current_cam_id)
        else:
            if err_callback:
                return err_callback()
            else:
                print("CamManger:set_current_cam: you have to set current camera cam first")
                return None

    def get_all(self):
        return [self.get(cam_id) for cam_id in self._cams_id]

camManger = CamManger()

class CamVideo(object):
    def __init__(self, cam):
        self.id = cam.id
        self.name = cam.name
        self.camProxy = proxyManger.get(cam.name + "camProxy")
        self.client = proxyManger.get(cam.name + "client")
        self.cameraMatrix = cam.get_cameraMatrix()
        self.distCoeffs = cam.get_distCoeffs()

    def get_image(self):
        # 获取一幅实时图像
        naoImage = self.camProxy.getImageRemote(self.client)
        if naoImage is None :
            if hasattr(self,"_image"):
                return self._image # 如果连接失败就返回缓存的图像
            else:
                return self.get_image() #
        else:
            imageWidth = naoImage[0]
            imageHeight = naoImage[1]
            array = np.frombuffer(naoImage[6],dtype=np.uint8)
            image = array.reshape((imageHeight, imageWidth,3))
            self._image = image # 图像缓存
            return image

    def frame_gener(self,scale=1.0,real=False):
        """视频流生成器"""
        while camManger.has(self.id):
            frame = self.get_frame(scale=scale,real=real)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n')
        raise StopIteration # 结束迭代

    def video_Response(self,scale=1.0,real=False):
        from flask import Response
        return Response(self.frame_gener(scale=scale,real=real),
                        mimetype='multipart/x-mixed-replace; boundary=frame')

    def get_frame(self,scale=1.0, real=False):
        if real:
            image = self.get_real_image()
        else:
            image = self.get_image()
        height, width = image.shape[:2]
        height = int(height*scale)
        width = int(width*scale)
        shape = (width, height)
        # res = cv2.resize(img,None,fx=scale, fy=scale, interpolation = cv2.INTER_CUBIC)
        image = cv2.resize(image,shape, interpolation = cv2.INTER_CUBIC)
        frame = np.array(cv2.imencode('.jpg',image)[1]).tostring()
        return frame

    def save_image(self,image_flag):
        assert hasattr(flag.image,image_flag)
        from .Image import Image
        cam = Cam.get(self.id)
        image = Image(cam=cam,name=image_flag)
        image = image.commit()
        image.save(self.get_image())


    def get_real_image(self):
        img = self.get_image()
        src = img
        cameraMatrix = self.cameraMatrix
        distCoeffs = self.distCoeffs
        dst = cv2.undistort(src,cameraMatrix,distCoeffs)
        return dst

# Nao 机器人相机模型
class Cam(db.Model):
    __tablename__ = "cams"
    id = db.Column(db.Integer,primary_key=True)
    name = db.Column(db.String(64),unique=True)
    position = db.Column(db.String(64))
    # 相机内参数
    fx = db.Column(db.Float,default=1.0)
    fy = db.Column(db.Float,default=1.0)
    cx = db.Column(db.Float,default=0.0)
    cy = db.Column(db.Float,default=0.0)
    k1 = db.Column(db.Float,default=0.0)
    k2 = db.Column(db.Float,default=0.0)
    p1 = db.Column(db.Float,default=0.0)
    p2 = db.Column(db.Float,default=0.0)
    k3 = db.Column(db.Float,default=0.0)
    # 标定单位
    # 相机中的1代表实际中的多少 cm,与标定时的标定板所用的单位有关
    unit = db.Column(db.Float,default=1.0)
    # 相关关联的对象
    nao_id = db.Column(db.Integer,db.ForeignKey('naos.id'))
    images = db.relationship('Image',backref='cam')

    def __init__(self,nao,position):
        from .Nao import Nao
        assert isinstance(nao,Nao)
        assert position in [u'top',u'bottom']
        self.nao = nao
        self.nao_id = nao.id
        self.position = position
        self.name = u'_'.join([u'python',self.nao.name,self.position])

    def __repr__(self):
        return "<Cam %s>" % (self.id)

    @classmethod
    def get(cls,id):
        return cls.query.get(id)

    @classmethod
    def get_by_name(cls,name):
        out = cls.query.filter_by(name=name).first()
        return out

    def get_position(self):
        return str(self.position)

    def connect(self):
        if not proxyManger.has(self.name + "camProxy"):
            proxyManger.add(self.name + "camProxy",self.nao.get_camProxy())
        if not proxyManger.has(self.name + "client"):
            camProxy = proxyManger.get(self.name + "camProxy")
            if self.position == u'top':
                self.index = vision_definitions.kTopCamera
            elif self.position == u'bottom':
                self.index = vision_definitions.kBottomCamera
            #resolution = vision_definitions.kVGA    # 分辨率 VGA (640x480)
            resolution = vision_definitions.kQVGA    # 分辨率 QVGA (320x240)
            FPS = 25
            #resolution = vision_definitions.k4VGA    # 分辨率 4VGA (1280*960)
            #FPS = 10
            colorSpace = vision_definitions.kBGRColorSpace   # BGR 色域

            prams = (str(self.name),self.index,resolution,colorSpace,FPS)
            proxyManger.add(self.name + "client",camProxy.subscribeCamera(*prams))

    def stop(self):
        camProxy = proxyManger.get(self.name + "camProxy")
        client = proxyManger.get(self.name + "client")
        camProxy.unsubscribe(client)

    def get_images_by_flag(self,image_flag):
        out_list = []
        for image in self.images:
            if image.name == image_flag:
                out_list.append(image)
        return out_list

    def calibration(self,w=12,h=9,unit=2):
        # 迭代终止标准
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        # 准备目标点，三维空间中的，以棋盘格的为单位长度
        objp = np.zeros((h*w,3), np.float32)
        objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
        # 从所有图像中储存所有的目标点和图像点
        objpoints = [] # 3d point in real world space
        imgpoints = [] # 2d points in image plane.

        from .Image import Image
        images = Image.get_cam_flag(self.id,flag.image.calibration)
        for image in images:
            img = image.get_image()
            gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
            # 寻找标定角点
            ret, corners = cv2.findChessboardCorners(gray, (w,h),None)
            # 如果找到角点，添加目标点和图像点
            if ret == True:
                objpoints.append(objp)
                cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
                imgpoints.append(corners)
        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
        # 设定相机内参数
        self.fx,self.fy,self.cx,self.cy = mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] # 相机矩阵
        self.k1,self.k2,self.p1,self.p2,self.k3 = dist[0] # 畸变矫正参数
        self.unit = 2
        db.session.add(self)
        #camManger.commit(self.id)

    def get_cameraMatrix(self):
        mtx = np.eye(3)
        mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] = self.fx,self.fy,self.cx,self.cy
        return mtx

    def get_distCoeffs(self):
        dist = np.zeros((1,5))
        dist[0] = self.k1,self.k2,self.p1,self.p2,self.k3
        return dist